#include <mathematics/Matrix3.h>

namespace ne
{
    const Matrix3 Matrix3::ZERO(
        0.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 0.0f);

    const Matrix3 Matrix3::IDENTITY(
        1.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f,
        0.0f, 0.0f, 1.0f);

    bool Matrix3::operator == (const Matrix3 &m) const
    {
        return (mData[0][0] == m.mData[0][0] &&
                mData[0][1] == m.mData[0][1] &&
                mData[0][2] == m.mData[0][2] &&
                mData[1][0] == m.mData[1][0] &&
                mData[1][1] == m.mData[1][1] &&
                mData[1][2] == m.mData[1][2] &&
                mData[2][0] == m.mData[2][0] &&
                mData[2][1] == m.mData[2][1] &&
                mData[2][2] == m.mData[2][2]);
    }

    bool Matrix3::operator != (const Matrix3 &m) const
    {
        return mData[0][0] != m.mData[0][0] ||
               mData[0][1] != m.mData[0][1] ||
               mData[0][2] != m.mData[0][2] ||
               mData[1][0] != m.mData[1][0] ||
               mData[1][1] != m.mData[1][1] ||
               mData[1][2] != m.mData[1][2] ||
               mData[2][0] != m.mData[2][0] ||
               mData[2][1] != m.mData[2][1] ||
               mData[2][2] != m.mData[2][2];
    }

    Matrix3 Matrix3::operator + (const Matrix3 &m) const
    {
        return Matrix3(
            mData[0][0] + m.mData[0][0],
            mData[0][1] + m.mData[0][1],
            mData[0][2] + m.mData[0][2],
            mData[1][0] + m.mData[1][0],
            mData[1][1] + m.mData[1][1],
            mData[1][2] + m.mData[1][2],
            mData[2][0] + m.mData[2][0],
            mData[2][1] + m.mData[2][1],
            mData[2][2] + m.mData[2][2]);
    }

    Matrix3 Matrix3::operator - (const Matrix3 &m) const
    {
        return Matrix3(
            mData[0][0] - m.mData[0][0],
            mData[0][1] - m.mData[0][1],
            mData[0][2] - m.mData[0][2],
            mData[1][0] - m.mData[1][0],
            mData[1][1] - m.mData[1][1],
            mData[1][2] - m.mData[1][2],
            mData[2][0] - m.mData[2][0],
            mData[2][1] - m.mData[2][1],
            mData[2][2] - m.mData[2][2]);
    }

    Matrix3 Matrix3::operator * (const Matrix3 &m) const
    {
        return Matrix3(
            mData[0][0] * m.mData[0][0] + mData[0][1] * m.mData[1][0] + mData[0][2] * m.mData[2][0],
            mData[0][0] * m.mData[0][1] + mData[0][1] * m.mData[1][1] + mData[0][2] * m.mData[2][1],
            mData[0][0] * m.mData[0][2] + mData[0][1] * m.mData[1][2] + mData[0][2] * m.mData[2][2],
            mData[1][0] * m.mData[0][0] + mData[1][1] * m.mData[1][0] + mData[1][2] * m.mData[2][0],
            mData[1][0] * m.mData[0][1] + mData[1][1] * m.mData[1][1] + mData[1][2] * m.mData[2][1],
            mData[1][0] * m.mData[0][2] + mData[1][1] * m.mData[1][2] + mData[1][2] * m.mData[2][2],
            mData[2][0] * m.mData[0][0] + mData[2][1] * m.mData[1][0] + mData[2][2] * m.mData[2][0],
            mData[2][0] * m.mData[0][1] + mData[2][1] * m.mData[1][1] + mData[2][2] * m.mData[2][1],
            mData[2][0] * m.mData[0][2] + mData[2][1] * m.mData[1][2] + mData[2][2] * m.mData[2][2]);
    }

    Matrix3 Matrix3::operator - () const
    {
        return Matrix3(
            -mData[0][0], -mData[0][1], -mData[0][2],
            -mData[1][0], -mData[1][1], -mData[1][2],
            -mData[2][0], -mData[2][1], -mData[2][2]);
    }

    Vector3 Matrix3::operator * (const Vector3 &v) const
    {
        return Vector3(
            mData[0][0] * v.x + mData[0][1] * v.y + mData[0][2] * v.z,
            mData[1][0] * v.x + mData[1][1] * v.y + mData[1][2] * v.z,
            mData[2][0] * v.x + mData[2][1] * v.y + mData[2][2] * v.z);
    }

    Vector3 operator * (const Vector3 &v, const Matrix3 &m)
    {
        return Vector3(
            v.x * m[0][0] + v.y * m[1][0] + v.z * m[2][0],
            v.x * m[0][1] + v.y * m[1][1] + v.z * m[2][1],
            v.x * m[0][2] + v.y * m[1][2] + v.z * m[2][2]);
    }

    Matrix3 Matrix3::operator * (const real f) const
    {
        return Matrix3(
            mData[0][0] * f, mData[0][1] * f, mData[0][2] * f,
            mData[1][0] * f, mData[1][1] * f, mData[1][2] * f,
            mData[2][0] * f, mData[2][1] * f, mData[2][2] * f);
    }

    Matrix3 operator * (const real f, const Matrix3 &m)
    {
        return Matrix3(
            f * m[0][0], f * m[0][1], f * m[0][2],
            f * m[1][0], f * m[1][1], f * m[1][2],
            f * m[2][0], f * m[2][1], f * m[2][2]);
    }

    void Matrix3::transpose(Matrix3 &m) const
    {
        m.mData[0][0] = mData[0][0];
        m.mData[0][1] = mData[1][0];
        m.mData[0][2] = mData[2][0];
        m.mData[1][0] = mData[0][1];
        m.mData[1][1] = mData[1][1];
        m.mData[1][2] = mData[2][1];
        m.mData[2][0] = mData[0][2];
        m.mData[2][1] = mData[1][2];
        m.mData[2][2] = mData[2][2];
    }

    Matrix3 Matrix3::transpose() const
    {
        return Matrix3(
            mData[0][0], mData[1][0], mData[2][0],
            mData[0][1], mData[1][1], mData[2][1],
            mData[0][2], mData[1][2], mData[2][2]);
    }

    // M = adj(m) = transpose(c(m))
    void Matrix3::adjoint(Matrix3 &m) const
    {
        m.mData[0][0] = mData[1][1] * mData[2][2] - mData[1][2] * mData[2][1];
        m.mData[0][1] = mData[0][2] * mData[2][1] - mData[0][1] * mData[2][2];
        m.mData[0][2] = mData[0][1] * mData[1][2] - mData[0][2] * mData[1][1];
        m.mData[1][0] = mData[1][2] * mData[2][0] - mData[1][0] * mData[2][2];
        m.mData[1][1] = mData[0][0] * mData[2][2] - mData[0][2] * mData[2][0];
        m.mData[1][2] = mData[0][2] * mData[1][0] - mData[0][0] * mData[1][2];
        m.mData[2][0] = mData[1][0] * mData[2][1] - mData[1][1] * mData[2][0];
        m.mData[2][1] = mData[0][1] * mData[2][0] - mData[0][0] * mData[2][1];
        m.mData[2][2] = mData[0][0] * mData[1][1] - mData[0][1] * mData[1][0];
    }

    Matrix3 Matrix3::adjoint() const
    {
        return Matrix3(
            mData[1][1] * mData[2][2] - mData[1][2] * mData[2][1],
            mData[0][2] * mData[2][1] - mData[0][1] * mData[2][2],
            mData[0][1] * mData[1][2] - mData[0][2] * mData[1][1],
            mData[1][2] * mData[2][0] - mData[1][0] * mData[2][2],
            mData[0][0] * mData[2][2] - mData[0][2] * mData[2][0],
            mData[0][2] * mData[1][0] - mData[0][0] * mData[1][2],
            mData[1][0] * mData[2][1] - mData[1][1] * mData[2][0],
            mData[0][1] * mData[2][0] - mData[0][0] * mData[2][1],
            mData[0][0] * mData[1][1] - mData[0][1] * mData[1][0]);
    }

    bool Matrix3::inverse(Matrix3 &m) const
    {
        // Invert a 3x3 using cofactors.  This is about 8 times faster than
        // the Numerical Recipes code which uses Gaussian elimination.
        adjoint(m);

        real det = mData[0][0] * m.mData[0][0] +
                   mData[0][1] * m.mData[1][0] +
                   mData[0][2] * m.mData[2][0];

        if (det == 0.0f)
        {
            m.identity();
            return false;
        }

        real invDet = 1.0f / det;
        m.mData[0][0] *= invDet;
        m.mData[0][1] *= invDet;
        m.mData[0][2] *= invDet;
        m.mData[1][0] *= invDet;
        m.mData[1][1] *= invDet;
        m.mData[1][2] *= invDet;
        m.mData[2][0] *= invDet;
        m.mData[2][1] *= invDet;
        m.mData[2][2] *= invDet;

        return true;
    }

    Matrix3 Matrix3::inverse() const
    {
        Matrix3 resM;
        inverse(resM);
        return resM;
    }

    real Matrix3::determinant() const
    {
        real f0 = mData[1][1] * mData[2][2] - mData[1][2] * mData[2][1];
        real f1 = mData[1][2] * mData[2][0] - mData[1][0] * mData[2][2];
        real f2 = mData[1][0] * mData[2][1] - mData[1][1] * mData[2][0];
        return mData[0][0] * f0 + mData[0][1] * f1 + mData[0][2] * f2;
    }

    void Matrix3::orthonormalize()
    {
        real len = std::sqrt(mData[0][0] * mData[0][0] +
                             mData[0][1] * mData[0][1] +
                             mData[0][2] * mData[0][2]);

        real invLen = (len == 0.0f) ? 0.0f : 1.0f / len;

        mData[0][0] *= invLen;
        mData[0][1] *= invLen;
        mData[0][2] *= invLen;

        // compute q1
        real fDot0 = mData[0][0] * mData[1][0] +
                     mData[0][1] * mData[1][1] +
                     mData[0][2] * mData[1][2];

        mData[1][0] -= fDot0 * mData[0][0];
        mData[1][1] -= fDot0 * mData[0][1];
        mData[1][2] -= fDot0 * mData[0][2];

        len = std::sqrt(mData[1][0] * mData[1][0] +
                        mData[1][1] * mData[1][1] +
                        mData[1][2] * mData[1][2]);

        invLen = (len == 0.0f) ? 0.0f : 1.0f / len;

        mData[1][0] *= invLen;
        mData[1][1] *= invLen;
        mData[1][2] *= invLen;

        // compute q2
        real fDot1 = mData[1][0] * mData[2][0] +
                     mData[1][1] * mData[2][1] +
                     mData[1][2] * mData[2][2];

        fDot0 = mData[0][0] * mData[2][0] +
                mData[0][1] * mData[2][1] +
                mData[0][2] * mData[2][2];

        mData[2][0] -= fDot0 * mData[0][0] + fDot1 * mData[1][0];
        mData[2][1] -= fDot0 * mData[0][1] + fDot1 * mData[1][1];
        mData[2][2] -= fDot0 * mData[0][2] + fDot1 * mData[1][2];

        len = std::sqrt(mData[2][0] * mData[2][0] +
                        mData[2][1] * mData[2][1] +
                        mData[2][2] * mData[2][2]);

        invLen = (len == 0.0f) ? 0.0f : 1.0f / len;

        mData[2][0] *= invLen;
        mData[2][1] *= invLen;
        mData[2][2] *= invLen;
    }

    void Matrix3::fromAxes(const Vector3 &vX, const Vector3 &vY, const Vector3 &vZ)
    {
        setRow(0, vX);
        setRow(1, vY);
        setRow(2, vZ);
    }

    void Matrix3::toAxes(Vector3 &vX, Vector3 &vY, Vector3 &vZ) const
    {
        vX = getRow(0);
        vY = getRow(1);
        vZ = getRow(2);
    }

    void Matrix3::fromAxisAngle(const Vector3 &axis, const real a)
    {
        Vector3 unitV = axis.getUnit();
        real fCos = std::cos(a);
        real fSin = std::sin(a);
        real fOneMinusCos = 1.0f - fCos;
        real fX2 = unitV.x * unitV.x;
        real fY2 = unitV.y * unitV.y;
        real fZ2 = unitV.z * unitV.z;
        real fXYM = unitV.x * unitV.y * fOneMinusCos;
        real fXZM = unitV.x * unitV.z * fOneMinusCos;
        real fYZM = unitV.y * unitV.z * fOneMinusCos;
        real fXSin = unitV.x * fSin;
        real fYSin = unitV.y * fSin;
        real fZSin = unitV.z * fSin;

        mData[0][0] = fX2 * fOneMinusCos + fCos;
        mData[0][1] = fXYM + fZSin;
        mData[0][2] = fXZM - fYSin;

        mData[1][0] = fXYM - fZSin;
        mData[1][1] = fY2 * fOneMinusCos + fCos;
        mData[1][2] = fYZM + fXSin;

        mData[2][0] = fXZM + fYSin;
        mData[2][1] = fYZM - fXSin;
        mData[2][2] = fZ2 * fOneMinusCos + fCos;
    }

    bool Matrix3::toAxisAngle(Vector3 &axis, real &a) const
    {
        real fTrace = mData[0][0] + mData[1][1] + mData[2][2];
        real fCos = 0.5f * (fTrace - 1.0f);
        a = std::acos(fCos);  // in [0,PI]

        if (a > 0.0f)
        {
            if (a < Math::PI)
            {
                axis.x = mData[1][2] - mData[2][1];
                axis.y = mData[2][0] - mData[0][2];
                axis.z = mData[0][1] - mData[1][0];
                axis.normalize();
            }
            else
            {
                // angle is PI
                real fHalfInverse;
                if (mData[0][0] >= mData[1][1])
                {
                    // r00 >= r11
                    if (mData[0][0] >= mData[2][2])
                    {
                        // r00 is maximum diagonal term
                        axis.x = 0.5f * std::sqrt(mData[0][0] - mData[1][1] -
                                                  mData[2][2] + 1.0f);
                        fHalfInverse = 0.5f / axis.x;
                        axis.y = fHalfInverse * mData[1][0];
                        axis.z = fHalfInverse * mData[2][0];
                    }
                    else
                    {
                        // r22 is maximum diagonal term
                        axis.z = 0.5f * std::sqrt(mData[2][2] - mData[0][0] -
                                                  mData[1][1] + 1.0f);
                        fHalfInverse = 0.5f / axis.z;
                        axis.x = fHalfInverse * mData[2][0];
                        axis.y = fHalfInverse * mData[2][1];
                    }
                }
                else
                {
                    // r11 > r00
                    if (mData[1][1] >= mData[2][2])
                    {
                        // r11 is maximum diagonal term
                        axis.y = 0.5f * std::sqrt(mData[1][1] - mData[0][0] -
                                                  mData[2][2] + 1.0f);
                        fHalfInverse  = 0.5f / axis.y;
                        axis.x = fHalfInverse * mData[1][0];
                        axis.z = fHalfInverse * mData[2][1];
                    }
                    else
                    {
                        // r22 is maximum diagonal term
                        axis.z = 0.5f * std::sqrt(mData[2][2] - mData[0][0] -
                                                  mData[1][1] + 1.0f);
                        fHalfInverse = 0.5f / axis.z;
                        axis.x = fHalfInverse * mData[2][0];
                        axis.y = fHalfInverse * mData[2][1];
                    }
                }
            }
            return true;
        }

        // The angle is 0 and the matrix is the identity.  Any axis will
        // work, so just use the x-axis.
        axis.x = 1.0f;
        axis.y = 0.0f;
        axis.z = 0.0f;
        return false;
    }

    void Matrix3::fromEulerAngles(const real yaw, const real pitch, const real roll)
    {
        real fCos = std::cos(pitch);
        real fSin = std::sin(pitch);
        Matrix3 kXMat(1.0f, 0.0f,   0.0f,
                      0.0f, fCos,  fSin,
                      0.0f, -fSin, fCos);

        fCos = std::cos(yaw);
        fSin = std::sin(yaw);
        Matrix3 kYMat(fCos, 0.0f, -fSin,
                      0.0f,  1.0f, 0.0f,
                      fSin, 0.0f, fCos);

        fCos = std::cos(roll);
        fSin = std::sin(roll);
        Matrix3 kZMat(fCos,  fSin, 0.0f,
                      -fSin, fCos, 0.0f,
                      0.0f,   0.0f,  1.0f);

        *this = (kZMat * kXMat) * kYMat;
    }

    bool Matrix3::toEulerAngles(real &yaw, real &pitch, real &roll) const
    {
        // rot =  cy*cz+sx*sy*sz   cx*sz  -cz*sy+cy*sx*sz
        //       -cy*sz+cz*sx*sy   cx*cz   cy*cz*sx+sy*sz
        //        cx*sy           -sx      cx*cy

        pitch = std::asin(-mData[2][1]);     // in [-HALF_PI, HALF_PI]
        if (pitch < Math::HALF_PI)
        {
            if (pitch > -Math::HALF_PI)
            {
                yaw = std::atan2(mData[2][0], mData[2][2]);
                roll = std::atan2(mData[0][1], mData[1][1]);
                return true;
            }

            // WARNING.  Not a unique solution.
            real fRmY = std::atan2(-mData[1][0], mData[0][0]);
            roll = 0.0f;  // any angle works
            yaw = roll - fRmY;
            return false;
        }

        // WARNING.  Not a unique solution.
        real fRpY = std::atan2(-mData[1][0], mData[0][0]);
        roll = 0.0f;  // any angle works
        yaw = fRpY - roll;
        return false;
    }

    void Matrix3::fromAxisXAngle(const real a)
    {
        real fCos = std::cos(a);
        real fSin = std::sin(a);
        mData[0][0] = 1.0f; mData[0][1] = 0.0f; mData[0][2] = 0.0f;
        mData[1][0] = 0.0f; mData[1][1] = fCos; mData[1][2] = -fSin;
        mData[2][0] = 0.0f; mData[2][1] = fSin; mData[2][2] = fCos;
    }

    void Matrix3::fromAxisYAngle(const real a)
    {
        real fCos = std::cos(a);
        real fSin = std::sin(a);
        mData[0][0] = fCos;  mData[0][1] = 0.0f; mData[0][2] = fSin;
        mData[1][0] = 0.0f;  mData[1][1] = 1.0f; mData[1][2] = 0.0f;
        mData[2][0] = -fSin; mData[2][1] = 0.0f; mData[2][2] = fCos;
    }

    void Matrix3::fromAxisZAngle(const real a)
    {
        real fCos = std::cos(a);
        real fSin = std::sin(a);
        mData[0][0] = fCos; mData[0][1] = -fSin; mData[0][2] = 0.0f;
        mData[1][0] = fSin; mData[1][1] = fCos;  mData[1][2] = 0.0f;
        mData[2][0] = 0.0f; mData[2][1] = 0.0f;  mData[2][2] = 1.0f;
    }

    void Matrix3::fromScaleAxisRate(const Vector3 &axis, const real rate)
    {
        Vector3 unitV = axis.getUnit();
        real fRateMinusOne = rate - 1.0f;
        real fX2M = unitV.x * unitV.x * fRateMinusOne;
        real fY2M = unitV.y * unitV.y * fRateMinusOne;
        real fZ2M = unitV.z * unitV.z * fRateMinusOne;
        real fXYM = unitV.x * unitV.y * fRateMinusOne;
        real fXZM = unitV.x * unitV.z * fRateMinusOne;
        real fYZM = unitV.y * unitV.z * fRateMinusOne;

        mData[0][0] = 1.0f + fX2M;
        mData[0][1] = fXYM;
        mData[0][2] = fXZM;

        mData[1][0] = fXYM;
        mData[1][1] = 1.0f + fY2M;
        mData[1][2] = fYZM;

        mData[2][0] = fXZM;
        mData[2][1] = fYZM;
        mData[2][2] = 1.0f + fZ2M;
    }

    void Matrix3::fromOrthoProjectionNormal(const Vector3 &norm)
    {
        Vector3 unitV = norm.getUnit();
        real fX2 = unitV.x * unitV.x;
        real fY2 = unitV.y * unitV.y;
        real fZ2 = unitV.z * unitV.z;
        real fXY = unitV.x * unitV.y;
        real fXZ = unitV.x * unitV.z;
        real fYZ = unitV.y * unitV.z;

        mData[0][0] = 1.0f - fX2;
        mData[0][1] = -fXY;
        mData[0][2] = -fXZ;

        mData[1][0] = -fXY;
        mData[1][1] = 1.0f - fY2;
        mData[1][2] = -fYZ;

        mData[2][0] = -fXZ;
        mData[2][1] = -fYZ;
        mData[2][2] = 1.0f - fZ2;
    }

    void Matrix3::fromReflectionNormal(const Vector3 &norm)
    {
        Vector3 unitV = norm.getUnit();
        real fX2 = unitV.x * unitV.x;
        real fY2 = unitV.y * unitV.y;
        real fZ2 = unitV.z * unitV.z;
        real fXY = unitV.x * unitV.y;
        real fXZ = unitV.x * unitV.z;
        real fYZ = unitV.y * unitV.z;

        mData[0][0] = 1.0f - 2.0f * fX2;
        mData[0][1] = -2.0f * fXY;
        mData[0][2] = -2.0f * fXZ;

        mData[1][0] = -2.0f * fXY;
        mData[1][1] = 1.0f - 2.0f * fY2;
        mData[1][2] = -2.0f * fYZ;

        mData[2][0] = -2.0f * fXZ;
        mData[2][1] = -2.0f * fYZ;
        mData[2][2] = 1.0f - 2.0f * fZ2;
    }

    std::ostream& operator << (std::ostream &o, const Matrix3 &m)
    {
        o << "Matrix3(" << m[0][0] << ", \t" << m[0][1] << ", \t" << m[0][2] <<
           "\n        " << m[1][0] << ", \t" << m[1][1] << ", \t" << m[1][2] <<
           "\n        " << m[2][0] << ", \t" << m[2][1] << ", \t" << m[2][2] << ")";
        return o;
    }
}
